#include <stdlib.h>
#include <GL/glut.h>
#include <iostream>
#include <cmath>
#include "Shape.h"
#include "Trajectory.h"
#include "iostream"

using namespace std;
Shape::Shape()
{
	redCol = 0;
	greenCol = 0;
	blueCol = 0;
	vert3d = NULL;

	indices = NULL;
	indicesFace1 = NULL;
	indicesFace2 = NULL;

	normals = NULL;
	normalsFace1 = NULL;
	normalsFace2 = NULL;

	numVert = 0;
	indexSize = 0;
	faceIndexes = 0;

	CONCAVE = 1;
	CONVEX = 2;
	CW = 3;
	CCW = 4;

}

void Shape::render() const
{
	glColor3f(redCol, greenCol, blueCol);
	glLineWidth(3.0f);
	glBegin(GL_LINE_LOOP);
	for( unsigned int i = 0; i < nVertices(); ++i)
	{
		glVertex2f( _vertices[i].x, _vertices[i].y );
	}
	
	glEnd();
}

void Shape::updateNextVertex(const Vec2& vertex)
{
	_nextVertex = vertex;
}

void Shape::addNextVertex()
{
	// Add the vertex to the end of the vector
	_vertices.push_back( _nextVertex );
	std::cout << _nextVertex.x << "," << _nextVertex.y << std::endl;
}

const Vec2& Shape::vertex(unsigned int num) const
{
	return _vertices[num];
}

unsigned int Shape::nVertices() const
{
	return (unsigned int)_vertices.size();
}

// Stores the Shape
void Shape::save( std::ostream& output )
{
	// Write the number of points
	output << _vertices.size() << std::endl;
	for(unsigned int i = 0; i < _vertices.size(); ++i)
	{
		output << _vertices[i].x << " " << _vertices[i].y << std::endl;
	}
}

// Loads a Shape
void Shape::load( std::istream& input )
{
	// Clear the previous vertices (if any)
	_vertices.clear();

	unsigned int total;
	input >> total;

	for(unsigned int i = 0; i < total; ++i)
	{
		float x, y;
		input >> x >> y;
		_vertices.push_back( Vec2(x, y) );
	}
}

void Shape::deleteAll(){
	_vertices.clear();

	if(vert3d)			delete[] vert3d;
	if(indices)			delete[] indices;
	if(indicesFace1)	delete[] indicesFace1;
	if(indicesFace2)	delete[] indicesFace2;
	if(normals)			delete[] normals;
	if(normalsFace1)	delete[] normalsFace1;
	if(normalsFace2)	delete[] normalsFace2;


	vert3d = NULL;

	indices = NULL;
	indicesFace1 = NULL;
	indicesFace2 = NULL;

	normals = NULL;
	normalsFace1 = NULL;
	normalsFace2 = NULL;

	numVert = 0;

}

void Shape::setColor(float red, float green, float blue)
{
	redCol = red;
	greenCol = green;
	blueCol = blue;
}

void Shape::deleteLastVertex()
{
	if(!_vertices.empty())
		_vertices.pop_back();
}	

// Yup. Takes a trayectory and it extrudes the shape.
void Shape::Create3DObjectFromTrajectory(Trajectory * traj)
{
	// Calculate the size of the coord vector
	const int numCoordPerVert = 3;
	numVert   = traj->nVertices() * nVertices();
	const int numCoords = numVert * numCoordPerVert;

	vert3d = new GLfloat[numCoords];
	int vCounter = 0;

	// Cheking for CCW or CW

	Vec2 * vect = new Vec2 [nVertices()];
	copy( _vertices.begin(), _vertices.end(), vect);

	// Fill the vertex array
	for(unsigned int i = 0; i < traj->nVertices(); i++)
	{
		for(unsigned int j = 0; j < nVertices(); j++)
		{
			GLfloat xs = vertex(j).x;
			GLfloat xt = traj->vertex(i).x;
			GLfloat x = xs+xt;

			GLfloat y = traj->vertex(i).y;

			GLfloat z = vertex(j).y;

			vert3d[vCounter] = x; vCounter++;
			vert3d[vCounter] = y; vCounter++;
			vert3d[vCounter] = z; vCounter++;
		}
	}

	// Fill the index array
	int a,b,c,d;
	faceIndexes = 2*nVertices();
	indexSize =  (traj->nVertices()-1)*nVertices()*4 + faceIndexes;
	indices = new GLint[indexSize];
	int iCounter = 0;

	int n=nVertices();
	
	// Check if the user defined the shape in CW or CCW
	int convexQ  =  Convex(vect,nVertices());
	if(convexQ == CONCAVE) cout << "CONCAVE" << endl;
	else if(convexQ == CONVEX) cout << "CONVEX" << endl;
	else cout << "Error on Convex" << endl;

	ccwQ =CounterClockWise(vect,nVertices(),convexQ);
	if(ccwQ == CCW) cout << "CCW" << endl;
	else if(ccwQ == CW) cout << "CW" << endl;
	else cout << "Error en CCW" << endl;

	// Depending on if it's CW or CCW, fill the index array in one direction or another. 
	// Just the poly between shapes, not the two ends
	if(ccwQ == CW){

		for(unsigned int tv = 0; tv < traj->nVertices()-1; tv++)	
		{
			for(unsigned int sv = 0; sv < nVertices(); sv++)
			{
				a = sv + tv*nVertices();
				if(sv == nVertices()-1)
				{
					// Last vertex, so we choose the first
					b = tv*nVertices();
				}
				else
				{
					b = sv+1 + tv*nVertices();
				}
				d = a + nVertices();
				c = b + nVertices();



				indices[iCounter] = a; iCounter++;
				indices[iCounter] = d; iCounter++;
				indices[iCounter] = c; iCounter++;
				indices[iCounter] = b; iCounter++;
				
				
			}
		}

		for(unsigned int sv = 0; sv < nVertices(); sv++)	
		{
			a = sv;
			indices[iCounter] = a; iCounter++;
		}

		for(int sv = nVertices()-1; sv >= 0; sv--)
		{
			a = (traj->nVertices()-1)*nVertices()+sv ;
			indices[iCounter] = a; iCounter++;
		}
	}
	else{
		// CCW
		for(unsigned int tv = 0; tv < traj->nVertices()-1; tv++)	
		{
			for(int sv = nVertices()-1; sv >= 0; sv--)
			{
				a = sv + tv*nVertices();
				if(sv == 0)
				{
					// Last vertex, so we choose the first
					b = (nVertices()-1)+tv*nVertices();
				}
				else
				{
					b = sv-1 + tv*nVertices();
				}
				d = a + nVertices();
				c = b + nVertices();



				indices[iCounter] = a; iCounter++;
				indices[iCounter] = d; iCounter++;
				indices[iCounter] = c; iCounter++;
				indices[iCounter] = b; iCounter++;
			}
		}

		for(int sv = nVertices()-1; sv >= 0; sv--)
		{
			a = sv;
			indices[iCounter] = a; iCounter++;
		}

		for(unsigned int sv = 0; sv < nVertices(); sv++)	
		{
			a = (traj->nVertices()-1)*nVertices()+sv ;
			indices[iCounter] = a; iCounter++;
		}
	}

	// For the faces
	indicesFace1 = new GLint[faceIndexes/2];
	indicesFace2 = new GLint[faceIndexes/2];
	int offset = indexSize - faceIndexes;
	for(int i = 0; i < faceIndexes/2; i++)
	{
		indicesFace1[i] = indices[i+offset];
		indicesFace2[i] = indices[i+offset+(faceIndexes/2)];
	}


	// Debug info
	int debug  = vert3d[2];

	cout << " Vertex list:" << endl;
	for(int i = 0; i < numCoords; i+=numCoordPerVert) 
	{
		printf("%d :" , (i/3) );
		printf("%f , %f, %f\n", vert3d[i],vert3d[i+1],vert3d[i+2]);
		
	}
	

	cout << endl << "Poly list: "<< endl;
	for(int i = 0; i < indexSize - faceIndexes; i+=4)
	{
		printf("%d :",(i/4));
		printf("%d, %d, %d, %d\n", indices[i],indices[i+1],indices[i+2],indices[i+3]);
	}
	printf("\n Faces:\n");
	for(int i = indexSize-faceIndexes; i < indexSize; i++)
	{
		printf("%d,",indices[i]);
		if(i == indexSize-(faceIndexes/2)-1) printf("\n");
	}
	
}

void Shape::render3D()
{
	if(!indices) return;

	// Render the quads between extrusions, and the two faces (top & bottom)
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, vert3d);
	glDrawElements(GL_QUADS, indexSize-faceIndexes, GL_UNSIGNED_INT, indices);
	glDrawElements(GL_POLYGON, faceIndexes/2, GL_UNSIGNED_INT, indicesFace1);
	glDrawElements(GL_POLYGON, faceIndexes/2, GL_UNSIGNED_INT, indicesFace2);
	glDisableClientState(GL_VERTEX_ARRAY);
}

void Shape::CreateNormalsFor3DObject(Trajectory * traj)
{
	int polygons=(indexSize-faceIndexes)/4;
	GLfloat* N=new GLfloat[(polygons+2)*3];
	int Ax,Ay,Az,Bx,By,Bz;
	int currentVertex;

	//el resto de normales
	for(int i=0;i<polygons;i++){
		 
		currentVertex=i*4;
		Ax=vert3d[indices[currentVertex]*3]-vert3d[indices[currentVertex+1]*3];
		Bx=vert3d[indices[currentVertex+2]*3]-vert3d[indices[currentVertex+1]*3];
		Ay=vert3d[indices[currentVertex]*3+1]-vert3d[indices[currentVertex+1]*3+1];
		By=vert3d[indices[currentVertex+2]*3+1]-vert3d[indices[currentVertex+1]*3+1];
		Az=vert3d[indices[currentVertex]*3+2]-vert3d[indices[currentVertex+1]*3+2];
		Bz=vert3d[indices[currentVertex+2]*3+2]-vert3d[indices[currentVertex+1]*3+2];
		
		N[i*3]=Ay*Bz-Az*By;
		N[i*3+1]=Ax*Bz-Az*Bx;
		N[i*3+2]=Ax*By-Ay*Bx;
	}

	//normales de las caras
	currentVertex=polygons*4;

	
	Ax=vert3d[indices[currentVertex]*3]-vert3d[indices[currentVertex+1]*3];
	Bx=vert3d[indices[currentVertex+1]*3]-vert3d[indices[currentVertex+2]*3];
	Ay=vert3d[indices[currentVertex]*3+1]-vert3d[indices[currentVertex+1]*3+1];
	By=vert3d[indices[currentVertex+1]*3+1]-vert3d[indices[currentVertex+2]*3+1];
	Az=vert3d[indices[currentVertex]*3+2]-vert3d[indices[currentVertex+1]*3+2];
	Bz=vert3d[indices[currentVertex+1]*3+2]-vert3d[indices[currentVertex+2]*3+2];
	
	N[polygons*3]=Ay*Bz-Az*By;
	N[polygons*3+1]=Ax*Bz-Az*Bx;
	N[polygons*3+2]=Ax*By-Ay*Bx;

	currentVertex=polygons*4+nVertices();
	
	Ax=vert3d[indices[currentVertex]*3]-vert3d[indices[currentVertex+1]*3];
	Bx=vert3d[indices[currentVertex+1]*3]-vert3d[indices[currentVertex+2]*3];
	Ay=vert3d[indices[currentVertex]*3+1]-vert3d[indices[currentVertex+1]*3+1];
	By=vert3d[indices[currentVertex+1]*3+1]-vert3d[indices[currentVertex+2]*3+1];
	Az=vert3d[indices[currentVertex]*3+2]-vert3d[indices[currentVertex+1]*3+2];
	Bz=vert3d[indices[currentVertex+1]*3+2]-vert3d[indices[currentVertex+2]*3+2];

	N[(polygons+1)*3]=Ay*Bz-Az*By;
	N[(polygons+1)*3+1]=Ax*Bz-Az*Bx;
	N[(polygons+1)*3+2]=Ax*By-Ay*Bx;

	for(int i=0;i<polygons+2;i++){
		float module= sqrt((float)(N[i*3]*N[i*3]+N[i*3+1]*N[i*3+1]+N[i*3+2]*N[i*3+2]));
		N[i*3]/=module;
		N[i*3+1]/=module;
		N[i*3+2]/=module;
	}

	normals=new GLfloat[numVert*3];
	
	for(int i=0; i<indexSize; i++){
		normals[i]=0.0;
	}

	if(ccwQ==CW){
		int polygonsPerLevel=polygons/(traj->nVertices()-1);
		for(unsigned int tv = 0; tv < traj->nVertices(); tv++)	
		{
			for(unsigned int sv = 0; sv < nVertices(); sv++)
			{
				int a=(sv + tv*nVertices())*3;

				if(sv==0 && tv==0){ //abajo,primer vertice del nivel
					normals[0]=(N[0]+N[(polygonsPerLevel-1)*3]+N[polygons*3])/3;
					normals[1]=(N[1]+N[(polygonsPerLevel-1)*3+1]+N[polygons*3+1])/3;
					normals[2]=(N[2]+N[(polygonsPerLevel-1)*3+2]+N[polygons*3+2])/3;
				}
				else if(tv==0){ //abajo
					normals[a]=(N[sv*3]+N[(sv-1)*3]+N[polygons*3])/3;
					normals[a+1]=(N[sv*3+1]+N[(sv-1)*3+1]+N[polygons*3+1])/3;
					normals[a+2]=(N[sv*3+2]+N[(sv-1)*3+2]+N[polygons*3+2])/3;
				}
				else if(sv==0 && tv!=traj->nVertices()-1){ //intermedio,primer vertice del nivel
					normals[a]=(N[tv*polygonsPerLevel*3]+N[tv*polygonsPerLevel*3+(polygonsPerLevel-1)*3]+N[(tv-1)*polygonsPerLevel*3]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3])/4;
					normals[a+1]=(N[tv*polygonsPerLevel*3+1]+N[tv*polygonsPerLevel*3+(polygonsPerLevel-1)*3+1]+N[(tv-1)*polygonsPerLevel*3+1]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+1])/4;
					normals[a+2]=(N[tv*polygonsPerLevel*3+2]+N[tv*polygonsPerLevel*3+(polygonsPerLevel-1)*3+2]+N[(tv-1)*polygonsPerLevel*3+2]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+2])/4;
				}
				else if(tv!=traj->nVertices()-1){  //intermedio
					normals[a]=(N[tv*polygonsPerLevel*3+sv*3]+N[tv*polygonsPerLevel*3+(sv-1)*3]+N[(tv-1)*polygonsPerLevel*3+sv*3]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+(sv-1)*3])/4;
					normals[a+1]=(N[tv*polygonsPerLevel*3+sv*3+1]+N[tv*polygonsPerLevel*3+(sv-1)*3+1]+N[(tv-1)*polygonsPerLevel*3+sv*3+1]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+(sv-1)*3+1])/4;
					normals[a+2]=(N[tv*polygonsPerLevel*3+sv*3+2]+N[tv*polygonsPerLevel*3+(sv-1)*3+2]+N[(tv-1)*polygonsPerLevel*3+sv*3+2]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+(sv-1)*3+2])/4;
				}
				else if(sv==0 && tv==traj->nVertices()-1){ //arriba,primer vertice del nivel
					normals[a]=(N[(tv-1)*polygonsPerLevel*3]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3]+N[tv*polygonsPerLevel*3])/3;
					normals[a+1]=-(N[(tv-1)*polygonsPerLevel*3+1]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+1]+N[tv*polygonsPerLevel*3+1])/3;
					normals[a+2]=(N[(tv-1)*polygonsPerLevel*3+2]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+2]+N[tv*polygonsPerLevel*3+2])/3;
				}
				else if(tv==traj->nVertices()-1){ //arriba
					normals[a]=(N[(tv-1)*polygonsPerLevel*3+sv*3]+N[(tv-1)*polygonsPerLevel*3+(sv-1)*3]+N[tv*polygonsPerLevel*3])/3;
					normals[a+1]=-(N[(tv-1)*polygonsPerLevel*3+sv*3+1]+N[(tv-1)*polygonsPerLevel*3+(sv-1)*3+1]+N[tv*polygonsPerLevel*3+1])/3;
					normals[a+2]=(N[(tv-1)*polygonsPerLevel*3+sv*3+2]+N[(tv-1)*polygonsPerLevel*3+(sv-1)*3+2]+N[tv*polygonsPerLevel*3+2])/3;
				}
			}
		}
	}
	else{
		int polygonsPerLevel=polygons/(traj->nVertices()-1);
		for(unsigned int tv = 0; tv < traj->nVertices(); tv++)	
		{
			for(unsigned int sv = 0; sv < nVertices(); sv++)
			{
				int a=(sv + tv*nVertices())*3;

				if(sv==0 && tv==0){ //abajo,primer vertice del nivel
					normals[0]=(N[0]+N[(polygonsPerLevel-1)*3]+N[polygons*3])/3;
					normals[1]=(N[1]+N[(polygonsPerLevel-1)*3+1]+N[polygons*3+1])/3;
					normals[2]=(N[2]+N[(polygonsPerLevel-1)*3+2]+N[polygons*3+2])/3;
				}
				else if(tv==0){ //abajo
					normals[a]=(N[sv*3]+N[(sv-1)*3]+N[polygons*3])/3;
					normals[a+1]=(N[sv*3+1]+N[(sv-1)*3+1]+N[polygons*3+1])/3;
					normals[a+2]=(N[sv*3+2]+N[(sv-1)*3+2]+N[polygons*3+2])/3;
				}
				else if(sv==0 && tv!=traj->nVertices()-1){ //intermedio,primer vertice del nivel
					normals[a]=(N[tv*polygonsPerLevel*3]+N[tv*polygonsPerLevel*3+(polygonsPerLevel-1)*3]+N[(tv-1)*polygonsPerLevel*3]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3])/4;
					normals[a+1]=(N[tv*polygonsPerLevel*3+1]+N[tv*polygonsPerLevel*3+(polygonsPerLevel-1)*3+1]+N[(tv-1)*polygonsPerLevel*3+1]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+1])/4;
					normals[a+2]=(N[tv*polygonsPerLevel*3+2]+N[tv*polygonsPerLevel*3+(polygonsPerLevel-1)*3+2]+N[(tv-1)*polygonsPerLevel*3+2]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+2])/4;
				}
				else if(tv!=traj->nVertices()-1){  //intermedio
					normals[a]=(N[tv*polygonsPerLevel*3+sv*3]+N[tv*polygonsPerLevel*3+(sv-1)*3]+N[(tv-1)*polygonsPerLevel*3+sv*3]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+(sv-1)*3])/4;
					normals[a+1]=(N[tv*polygonsPerLevel*3+sv*3+1]+N[tv*polygonsPerLevel*3+(sv-1)*3+1]+N[(tv-1)*polygonsPerLevel*3+sv*3+1]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+(sv-1)*3+1])/4;
					normals[a+2]=(N[tv*polygonsPerLevel*3+sv*3+2]+N[tv*polygonsPerLevel*3+(sv-1)*3+2]+N[(tv-1)*polygonsPerLevel*3+sv*3+2]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+(sv-1)*3+2])/4;
				}
				else if(sv==0 && tv==traj->nVertices()-1){ //arriba,primer vertice del nivel
					normals[a]=(N[(tv-1)*polygonsPerLevel*3]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3]+N[tv*polygonsPerLevel*3])/3;
					normals[a+1]=(N[(tv-1)*polygonsPerLevel*3+1]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+1]+N[tv*polygonsPerLevel*3+1])/3;
					normals[a+2]=(N[(tv-1)*polygonsPerLevel*3+2]+N[(tv-1)*polygonsPerLevel*3+(polygonsPerLevel-1)*3+2]+N[tv*polygonsPerLevel*3+2])/3;
				}
				else if(tv==traj->nVertices()-1){ //arriba
					normals[a]=(N[(tv-1)*polygonsPerLevel*3+sv*3]+N[(tv-1)*polygonsPerLevel*3+(sv-1)*3]+N[tv*polygonsPerLevel*3])/3;
					normals[a+1]=(N[(tv-1)*polygonsPerLevel*3+sv*3+1]+N[(tv-1)*polygonsPerLevel*3+(sv-1)*3+1]+N[tv*polygonsPerLevel*3+1])/3;
					normals[a+2]=(N[(tv-1)*polygonsPerLevel*3+sv*3+2]+N[(tv-1)*polygonsPerLevel*3+(sv-1)*3+2]+N[tv*polygonsPerLevel*3+2])/3;
				}
			}
		}
	}


	cout << endl <<"normals not normalized (lol):" << endl;
	for(int i = 0; i < numVert*3; i+=3)
	{
		printf("%d: ", (i/3));
		printf("%f, %f, %f\n",normals[i],normals[i+1],normals[i+2]);
	}


	for(int i=0;i<numVert;i++){
		float module= sqrt((float)(normals[i*3]*normals[i*3]+normals[i*3+1]*normals[i*3+1]+normals[i*3+2]*normals[i*3+2]));
		normals[i*3]/=module;
		normals[i*3+1]/=module;
		normals[i*3+2]/=module;
	}

	printf("\n");

	cout << endl <<"N:" << endl;
	for(int i = 0; i < (polygons+2)*3; i+=3)
	{
		printf("%d: ", (i/3));
		printf("%f, %f, %f\n",N[i],N[i+1],N[i+2]);
	}
	printf("\n");


	cout << endl <<"normals:" << endl;
	for(int i = 0; i < numVert*3; i+=3)
	{
		printf("%d: ", (i/3));
		printf("%f, %f, %f\n",normals[i],normals[i+1],normals[i+2]);
	}

	delete[] N;
	
}

void Shape::render3DSolid()
{
	if(!indices) return;

	

	// Render the quads between extrusions, and the two faces (top & bottom)
	
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_VERTEX_ARRAY);
	

	glVertexPointer(3, GL_FLOAT, 0, vert3d);
	glNormalPointer(GL_FLOAT, 0, normals);

	glDrawElements(GL_QUADS, indexSize-faceIndexes, GL_UNSIGNED_INT, indices);
	glDrawElements(GL_POLYGON, faceIndexes/2, GL_UNSIGNED_INT, indicesFace1);
	glDrawElements(GL_POLYGON, faceIndexes/2, GL_UNSIGNED_INT, indicesFace2);
	
	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
}


/*
   Return whether a polygon in 2D is concave or convex
   return 0 for incomputables eg: colinear points
          CONVEX == 1
          CONCAVE == -1
   It is assumed that the polygon is simple
   (does not intersect itself or have holes)

   Copywright: Written by Paul Bourke
   Reference: http://local.wasp.uwa.edu.au/~pbourke/geometry/clockwise/index.html
*/
int Shape::Convex(Vec2 *p,int n)
{
   int i,j,k;
   int flag = 0;
   double z;

   if (n < 3)
      return(0);

   for (i=0;i<n;i++) {
      j = (i + 1) % n;
      k = (i + 2) % n;
      z  = (p[j].x - p[i].x) * (p[k].y - p[j].y);
      z -= (p[j].y - p[i].y) * (p[k].x - p[j].x);
      if (z < 0)
         flag |= 1;
      else if (z > 0)
         flag |= 2;
      if (flag == 3)
         return(CONCAVE);
   }
   if (flag != 0)
      return(CONVEX);
   else
      return(0);
}

// Returns if the shape is CW or CCW, depending on if its Concave or Convex
int Shape::CounterClockWise(Vec2 * p, int n, int shapeT)
{
	if(shapeT == CONVEX)
	{
		int i=0,j=1,k=2;
		int flag = 0;
		double z;

		z  = (p[j].x - p[i].x) * (p[k].y - p[j].y);
		z -= (p[j].y - p[i].y) * (p[k].x - p[j].x);

		if(z > 0 ) return CCW; else return CW;
	}

	else if(shapeT == CONCAVE)
	{
		int sum = 0;
		for (int i=0;i<n-1;i++) 
		{
			sum += p[i].x*p[i+1].y - p[i+1].x*p[i].y;
		}

		if(sum > 0 ) return CCW; else return CW;
	}

	else
	{
		cout << "Error in CCW" << endl;
		return -1;
	}
}

void Shape::DrawNormals(){
	for(int i=0; i<numVert;i++){
		glBegin(GL_LINES);
		glColor3f(0.9,0.0,0.0);
		glVertex3f(vert3d[i*3],vert3d[i*3+1],vert3d[i*3+2]);
		glColor3f(0.0,0.9,0.0);
		glVertex3f(vert3d[i*3]+normals[i*3]*2,vert3d[i*3+1]+normals[i*3+1]*2,vert3d[i*3+2]+normals[i*3+2]*2);
		glEnd();
	}
}


/*
glPushMatrix();

glPopMatrix();
*/